#include "elmo.h"
#include "stm32f2xx.h"
#include "can_transmit_queue.h"
//TODO
struct velocity_param
{
	int velocity;             //  平滑速度模式期望速度值
	int acc;               //  平滑速度模式加速度值
	int dec;               //  平滑速度模式减速度值
	int qs_dec;             //  平滑速度模式急停减速度值
};

struct position_param
{
    int position;
    int velocity;             
    int acc;               
    int dec;               
    int qs_dec;             // quick stop dec 
};

struct  torque_param
{
  int max_speed;        
  int torque;            		// current  
};
enum elmo_status
{
	READY_TO_SWITCHON  = 0,
	SWITCHED_ON        = 1,
	OPERATION_ENABLED  = 2,
	FAULT              = 3,
	VOLTAGE_ENABLED    = 4,
	QUICK_STOP         = 5,
	SWITCH_ON_DISABLED = 6
};

enum control_mode
{
	IDLE_MODE     = 0,
	POSITION_MODE = 1,
	VELOCITY_MODE = 2,
	TORQUE_MODE   = 3
};

struct elmo_struct
{
	uint8_t node_id; 				        //  elmo结点号
	enum elmo_status status;   
	int current_position;
	int current_velocity;
	int current_torque;
	enum control_mode mode;
    struct position_param position;       	//  平滑位置模式
    struct velocity_param velocity;       	//  平滑位置模式
    struct torque_param torque;       	//  平滑位置模式
};
static struct elmo_struct elmo = {1};
struct can_transmit_queue elmo_transmit_queue;

/* content of one can frame 
*|Control Word|index |Subindex|Data[1]||Data[2]|Data[3]|Data[4]|
*/


 void elmo_write(uint32_t index, uint8_t sub_index, int32_t param)
{
	CanTxMsg TxMessage;
	TxMessage.StdId = 0x600 + elmo.node_id;
	TxMessage.ExtId = 0x01;
	TxMessage.RTR = CAN_RTR_DATA;
	TxMessage.IDE = CAN_ID_STD;
	TxMessage.DLC = 8;
	TxMessage.Data[0]= 0x22;                    //  CAN报文前缀，控制字表示写
	TxMessage.Data[1]= index & 0xFF;              //  发送index低字节
	TxMessage.Data[2]= (index & 0xFF00)>>8;       //  发送index高字节
	TxMessage.Data[3]= sub_index;                //  发送Subindex
	TxMessage.Data[4]= param & 0xFF;
	TxMessage.Data[5]= (param & 0xFF00)>>8;
	TxMessage.Data[6]= (param & 0xFF0000)>>16;
	TxMessage.Data[7]= (param & 0xFF000000)>>24;  //  依次发送对象内容
	insert_queue(&elmo_transmit_queue, &TxMessage);
}

static void elmo_read(uint16_t index, uint8_t sub_index)
{
	CanTxMsg TxMessage;
	TxMessage.StdId = 0x600 + elmo.node_id;	  
	TxMessage.ExtId = 0x01;
	TxMessage.RTR = CAN_RTR_DATA;
	TxMessage.IDE = CAN_ID_STD;
	TxMessage.DLC = 8;
	TxMessage.Data[0]= 0x40;               
	TxMessage.Data[1]= index & 0xFF;         
	TxMessage.Data[2]= (index & 0xFF00)>>8;  
	TxMessage.Data[3]= sub_index;           
	TxMessage.Data[4]= 0;
	TxMessage.Data[5]= 0;
	TxMessage.Data[6]= 0;
	TxMessage.Data[7]= 0;   
	if(insert_queue(&elmo_transmit_queue, &TxMessage)==0)
    {
    }
	
}

void elmo_init()
{
    elmo_write(0x6040, 0x00, 0x80);  //  上升沿清除ElmoCrl错误
    elmo_write(0x6040, 0x00, 0x06);  //  Shutdown
    elmo_write(0x6040, 0x00, 0x07);  //  SwitchOn
    elmo_write(0x6040, 0x00, 0x0f);  //  Enable  Operation
    elmo_write(0x2f41, 0x00, 0x02);  //  启动PPM 
    elmo.status = OPERATION_ENABLED; //  debug 如果不用这句会怎么样
}

void elmo_init2()
{
  elmo_write(0x6040, 0x00, 0x80);               //  上升沿清除ElmoCrl错误
  elmo_write(0x6065, 0x00, MAX_F_ERR);          //  PPMode下当前位置和实际位置的最大允许误差 
  elmo_write(0x6072, 0x00, MAX_TORQUE);         //  设定用户允许的电机最大扭矩
  elmo_write(0x6073, 0x00, MAX_CURRENT);        //  设定用户允许掉的电机最大电流
  elmo_write(0x6075, 0x00, RATE_CURRENT);       //  电机的额定电流(mA)---扭矩模式下用的到
  elmo_write(0x6076, 0x00, RATE_TORQUET);       //  电机的额定扭矩(mNm)---扭矩模式下用的到
  elmo_write(0x607D, 0x01, MIN_P_LIMIT);        //  PPMode下最小位置极限
  elmo_write(0x607D, 0x02, MAX_P_LIMIT);        //  PPMode下最大位置极限
  elmo_write(0x607e, 0x00, POLARITY  );         //  位置、速度极性 (根据需要修改)
  elmo_write(0x607F, 0x00, MAX_SPEED);          //  PPMode和PVMode下所能达到的最大速度
  elmo_write(0x6083, 0x00, MAX_ACC);            //  最大加速度
  elmo_write(0x6084, 0x00, MAX_ACC);            //  最大减速度
  elmo_write(0x6085, 0x00, MAX_QDEC);           //  各种模式下,ElmoCrl出错时的最大急停减速度
  elmo_write(0x6086, 0x00, MOTION_PROFILE_TYPE);//  PPM实现方式;0:linear ramp;1:sin2 ramp     
  elmo_write(0x6088, 0x00, 0);                  //  PTM实现方式,ELMO只支持 linear ramp   
  elmo_write(0x2f41, 0x00, 0x02);               //  启动PPM 
}


static void elmo_status_convert(enum elmo_status aim_status)
{
	/* 动作为:switch on */
	if(elmo.status == READY_TO_SWITCHON && aim_status == SWITCHED_ON)
	{
		elmo_write(0x6040, 0x00, 0x07);     
		elmo.status = aim_status;
	}
	 /* 动作为:switch on +operation on */
	if(elmo.status==READY_TO_SWITCHON && aim_status==OPERATION_ENABLED)
	{
     	//SwitchOn
		elmo_write( 0x6040, 0x00, 0x07);    
        //Enable  Operation
		elmo_write( 0x6040, 0x00, 0x0f);   
		elmo.status = aim_status;
	}

	/* 动作 :operation on*/
    if(elmo.status==SWITCHED_ON && aim_status==OPERATION_ENABLED)
	{
        //Enable  Operation
		elmo_write( 0x6040, 0x00, 0x0f); 
		elmo.status = aim_status;
	}

	/* 动作为:清除错误 + shutdown + switch on + operation on */
	if(elmo.status == FAULT &&aim_status==OPERATION_ENABLED)
	{
        //清除错误
		elmo_write( 0x6040, 0x00, 0x80);
        //Shutdown
		elmo_write( 0x6040, 0x00, 0x06);
    	//SwitchOn
		elmo_write( 0x6040, 0x00, 0x07);  
    	//Enable  Operation
		elmo_write( 0x6040, 0x00, 0x0f);     
		elmo.status = aim_status;
    }

	if(elmo.status==SWITCH_ON_DISABLED && aim_status==OPERATION_ENABLED)
	{
       	//  Shutdown
		elmo_write( 0x6040, 0x00, 0x06);  
        //  SwitchOn
		elmo_write( 0x6040, 0x00, 0x07); 
     	//  Enable  Operation
		elmo_write( 0x6040, 0x00, 0x0f);    
		elmo.status = aim_status;
    }
	
	if(elmo.status == FAULT && aim_status == SWITCH_ON_DISABLED)   
	{
        // 清除错误
	  	elmo_write( 0x6040, 0x00, 0x80);   
		elmo.status = aim_status;
	}
}

void elmo_run_velocity(int32_t vel, int32_t acc, int32_t dec, int32_t qs_dec)
{
	enum elmo_status st = elmo_get_status();
    
	if(st != OPERATION_ENABLED)
	{
		elmo_status_convert(OPERATION_ENABLED);
	}
	if ( elmo.mode != VELOCITY_MODE)
	{
		elmo.mode = VELOCITY_MODE;
    	//Enter PVMode
		elmo_write( 0x6060, 0x00, 0x03);
    	//Enter PVMode
		//Enter PVMode
    }
	if (elmo.velocity.acc != acc) 
	{
		//设置加速度
		elmo.velocity.acc = acc;
		elmo_write(0x6083, 0x00, acc);
	}
	if (elmo.velocity.dec != dec) 
	{
 		//设置减速度
		elmo.velocity.dec  = dec;
		elmo_write( 0x6084, 0x00, dec);
	}
	if (elmo.velocity.qs_dec != qs_dec)  
	{
		//设置急停减速度
		elmo.velocity.qs_dec = qs_dec;
		elmo_write(0x6085, 0x00, qs_dec);
	}  
	//if (ElmoCrl->Mode.PVMode.Speed != TargetSpeed) //  设置目标速度，同时启动PVM
	{
		elmo.velocity.velocity = vel;
		elmo_write( 0x60FF,  0x00, vel);
	}
}		  
void elmo_run_position(int position, int vel, int acc, int dec, int qs_dec)
{
	enum elmo_status st = elmo_get_status();
    
	if(st != OPERATION_ENABLED)
	{
		elmo_status_convert(OPERATION_ENABLED);
	}
	if (elmo.mode != POSITION_MODE)
	{
		elmo.mode = POSITION_MODE;  
      //  Enter PPMode
		elmo_write( 0x6060, 0x00, 0x01);      
	}
	if (elmo.position.velocity != vel)       
	{
    	//设置最大速度
		elmo.position.velocity  = vel;
		elmo_write(0x6081, 0x00, vel);
	}
	if(elmo.position.acc != acc) 
	{
        //  设置加速度
		elmo.position.acc = acc;
		elmo_write( 0x6083, 0x00, acc);
	}
	if(elmo.position.dec != dec)     
	{ 
        //设置减速度
		elmo.position.dec = dec;
		elmo_write( 0x6084, 0x00, dec);
	}
	if (elmo.position.qs_dec != qs_dec)  
	{
 		//设置急停减速度
		elmo.position.qs_dec = qs_dec;
		elmo_write( 0x6085, 0x00, qs_dec);
	}
	//if(ElmoCrl->Mode.PPMode.Position != TargetPosition)      //  设置目标位置  
	{
		//注释掉上面一行防止不动的情况出现
		elmo.position.position = position;
		elmo_write(0x607A, 0x00, position);
        //Enable  Operation
		elmo_write(0x6040, 0x00, 0x0f); 
        //选择运行模式，同时启动PPM
		elmo_write(0x6040, 0x00, 0x3f);
	}  
}

/**
  * @brief 平滑力矩控制
  * @param ElmoCrl ELMO结构体
  * @param Torque 目标力矩值其实质是电流值
  *  单位是额定力矩的千分之一
  *  unit:per thousand of rated torque. 
  * @retval 无
  * @note
  *
  */
void elmo_run_torque(int torque)
{
	enum elmo_status st = elmo_get_status();

	if(st != OPERATION_ENABLED)
	{
		elmo_status_convert(OPERATION_ENABLED);
	}

	//先发送目标力矩再使能，避免迅速超速
	if (elmo.mode != TORQUE_MODE)  
	{
		elmo.torque.torque = torque;
		//设置期望的扭矩
		elmo_write(0x6071, 0x00, torque);
		elmo.mode = TORQUE_MODE;
 		//Enter PTMode    
		elmo_write(0x6060, 0x00, 0x04);    
	}
	else	// if (ElmoCrl->Mode.PTMode.Torque != TargetTorque)
	{
		elmo.torque.torque = torque;
		//设置期望的扭矩
		elmo_write(0x6071, 0x00, torque);
	}

}
void elmo_quick_stop()
{
	if(elmo.status != FAULT )
	{
    	//quick_stop
		elmo_write( 0x6040, 0x00, 0x02); 
		elmo.status = SWITCH_ON_DISABLED;
	}
	else
	{
		elmo_status_convert(SWITCH_ON_DISABLED);
	}
}

//for current 
int elmo_call_get_current()
{
	elmo_read(ELMO_TORQUE_INDEX, ELMO_SUBINDEX_0);
	return 0;
}

void elmo_update_current(int current)
{
	elmo.current_torque = current;
}

int elmo_get_current()
{
	return elmo.current_torque;
}

//for position
int elmo_call_get_position(void)
{
	elmo_read(ELMO_POSITION_INDEX, ELMO_SUBINDEX_0);		
	return 0;
}

void elmo_update_position(int position)
{
	elmo.current_position = position;
}

int elmo_get_position()
{
	return elmo.current_position;
}

//for velocity 
int elmo_call_get_velocity(void)
{
	elmo_read(ELMO_VELOCITY_INDEX, ELMO_SUBINDEX_0);		
	return 0;
}

void elmo_update_velocity(int velocity)
{
	elmo.current_velocity = velocity;
}

int elmo_get_velocity(void)
{
	return elmo.current_velocity;
}

//for status 
int elmo_call_get_status(void)
{
	elmo_read(ELMO_STATUS_INDEX, ELMO_SUBINDEX_0);
	return 0;
}

enum elmo_status elmo_get_status(void)
{
	return elmo.status;
}

void  elmo_update_status(int elmo_status)
{
	if(elmo_status == 0x00001218) 
    {
        elmo.status = FAULT;
    }
}
